X
Реквизиты статьи для указания в качестве источника:
М.В.Момот, П.М.Момот Использование микроконтроллера ESP32 совместно с гироприборами MPU-6050 и BNO055 при создании самобалансирующих роботов Инновационные технологии в машиностроении: сборник трудов X Международной научно-практической конференции / Юргинский технологический институт. – Томск : Изд-во Томского политехнического университета, 2019. − С. 255-264.
Использование микроконтроллера ESP32 совместно с гироприборами MPU-6050 и BNO055 при создании самобалансирующих роботов
М.В. Момот Юргинский технологический институт (филиал) Национального исследовательского
Томского политехнического университета, П.М. Момот студент группы 5А74 Инженерная школа энергетики НИ ТПУ
652055, Кемеровская обл., г. Юрга, ул. Ленинградская, 26
E-mail: momotmvu@yandex.ru
Аннотация
Статья посвящена анализу возможностей электронных приборов, таких как гироскоп, акселерометр, магнитометр. Приводятся результаты тестирования и действующие алгоритмы. Анализируются возможности их применения в разных вариантах расчетов. Делаются выводы по развитию возможностей и устранению недостатков полученной комплексной системы позиционирования робота.
Abstract
Article is devoted to the analysis of opportunities of electronic devices, such as gyroscope, accelerometer, magnetometer. Results of testing and the operating algorithms are given. Possibilities of their application in different options of calculations are analyzed. Conclusions on development of opportunities and elimination of shortcomings of the received complex system of positioning of the robot are drawn.
Ключевые слова: гироскоп, акселерометр, магнитометр, BNO055, MPU5060, самобалансирующий робот.
Keyword: gyroscope, accelerometer, magnetometer, BNO055, MPU5060, the self-balancing robot.
При попытке сбалансировать в неустойчивом верхнем положении двухколесного моторного робота, которого иногда называют обратным маятником, нужно решить задачу противодействия силам, стремящимся его уронить (начать балансировать). К таким силам в первую очередь относится сила тяжести. Простейший способ балансировки заключается в анализе угла наклона робота и создании пропорционального противодействия доработкой ходовой робота.
Рисунок 1.Этапы балансировки обратного маятника
Рассмотрим данную ситуацию на примере. На рисунке приведены варианты отклонений робота от неустойчивого равновесного состояния: Если робот из состояния а) склонился в состояние б), то следует предпринять определенное перемещение, направленное в сторону склонения, в результате чего робот, обладая инерционностью, будет возвращен в равновесное состояние. Конечно, важно не только направление противодействующего перемещения, но его скорость и продолжительность. Недостаточная скорость не позволит компенсировать скорость падения, и робот все равно упадет, тоже произойдет, если недостаточным будет длительность (об этом еще поговорим). Слишком высокая скорость и/или длительность перемещения может привести к обратному эффекту, когда робот проскочит точку равновесия и отклонится в обратную сторону (состояние г.).
Рисунок 2. Простейший алгоритм балансировки
Самая простая формула балансировки робота приведена ниже.
, (1)
где A – угол склонения робота (со знаком), Kp – коэффициент пропорциональности, Speedmotor – скорость компенсирующего движения.
Для того чтобы робот не упал, следует производить коррекцию его скорости регулярно с некоторой частотой (зависит от типов двигателей и высоты центра масс робота относительно поверхности движения). В случае робота описанного ниже его корректировка производится каждые 5 миллисекунд. Алгоритм балансировки будет выглядеть, как показано на рис.2. Не забываем, что угол наклона при склонении в разные стороны будет иметь разный знак, это подразумевает разное противодействующее направления вращения колес.
Последнее время, для подобных измерений используется комбинированный прибор: гироскоп, акселерометр, магнитометр BNO055, его использование значительно упрощает реализацию систем позиционирования робота, но он достаточно редок и отличается высокой стоимостью, поэтому для получения значений угла наклона робота будем рассматривать наиболее доступный на сегодняшний день комбинированный (гироскоп-акселерометр) MPU-6050.
Рисунок 3. Нахождение угла отклонения от вертикали (робот неподвижен)
При использовании электронного инерционного прибора MPU-6050 можно получить три вида измерений:
-
угловую скорость по трем осям, которая, после математической обработки, может служить для расчета углов поворота прибора;
-
проекцию вектора ускорения на три оси, которую можно применять для измерения положения прибора относительно направления вектора ускорения свободного падения;
-
температуру – можно использовать для расчета корректировок показаний прибора основанных на температурных дрейфах его параметров из предыдущих пунктов.
Казалось бы, для расчета угла наклона нужно знать только показания акселерометра, т.е. берем пару проекций (зависит от того как размещен прибор на роботе) и рассчитываем угол наклона робота. Пример использования проекции ускорения свободного падения на оси Z и Y приведен на рисунке.
Для точного измерения объект должен быть неподвижен или двигаться без ускорения. В этом случае на него действует только сила притяжения, и следственно прибор измеряет только проекции ускорения свободного падения на оси акселерометра.
На рис.3 изображен неподвижный объект (робот) на наклонной плоскости. Координатная ось Y акселерометра робота направлена вперед, ось Z — вниз под прямым углом к наклонной плоскости, ось X — к наблюдателю (не используется). Искомый угол отклонения объекта от вертикали находится по формуле:
(2)
где — проекция ускорения свободного падения на ось Y акселерометра, — проекция на ось Z.
Соберем два стенда и попытаемся получить угол наклона робота.
В качестве микроконтроллера можно использовать большинство плат из семейства Arduino, мы же остановимся на наиболее симпатичной, на сегодняшний день, плате на основе контроллера ESP32. Как подключить поддержку для ESP32 в ArduinoIDE можно прочитать по ссылке http://zizibot.ru/articles/electronics/mobile-robot-on-esp32-and-shagovikakh/ ).
Принципиальных отличий при подключении двух различных гироприборов: MPU-6050 и BNO055 нет, оба не требовательны к питанию (хорошо себя чувствуют при напряжении 3.3в), оба подключаются к шине обмена данными I2C.
|
|
|
Рисунок 4. Подключение MPU5060 к ESP32
Но, если MPU-6050, это только измерительный прибор, измеряющий угловую скорость и проекции вектора ускорения на оси прибора, то BNO055 содержит электронный магнитометр и встроенный микроконтроллер, который производит большинство промежуточных вычислений и может выдавать окончательные значения для использования в реальных задачах, например это могут быть углы Эйлера.
Тестирование функций BNO055 и более подробную информацию можно получить из статьи http://zizibot.ru/directory/sensor/bno055/
|
|
|
Рисунок 5. Подключение BNO055 к ESP32
Так как это стенды, то отдельного питания на них подавать не нужно, оно будет поступать по usb-кабелю служащему для программирования. Кабель должен быть качественный и, по возможности новый, способный передавать токи до 500mA без нагрева, это связано с требованиями по питанию контроллера ESP32.
Для работы с MPU-6050 воспользуемся библиотекой от Korneliusz Jarzebski: https://github.com/jarzebski/Arduino-MPU6050. Я не буду устанавливать данную библиотеку в ArduinoIDE стандартными средствами, а скопирую её в каталог с тестовой программой. Потребуется заменить #include < MPU6050.h> на #include "MPU6050.h" в основной программе и файле MPU6050.cpp. Также, в связи с измененным подключением функций I2C для ESP32, нужно выполнить корректировку кода в MPU6050.cpp, как показано на рисунке.
Рисунок 6. Внесенные изменения для работы с I2C
Теперь загрузим в ESP32 следующий код, он показывает значения углов наклона, используя данные от акселерометра MPU-6050.
Рисунок 7. Программа тестирования MPU-5060
Рисунок 8. Результаты работы программы
Немного изменим функцию loop, чтобы воспользоваться всторенным в ArduinoIDE плоттером последовательного соединения. Оставим только вычисления угла наклона вокруг оси Y конец строки после окончания.
Рисунок 9. Программа тестирования MPU-5060 адаптированная под построение графика
Анализ показывает значительные колебания полученных значений. Для нижнего рисунка я плавно поворачивал MPU-6050 вокруг оси Y, но видны сильные колебания результирующего угла, это «шум», который не позволит нам получать адекватные значения угла, используя только данные акселерометра.
Рисунок 10. Результаты работы программы в виде графика (1)
Следующий рисунок показывает, как ведет себя неподвижный акселерометр, разброс значений достигает двух градусов, что для балансирующего робота не допустимо.
Рисунок 11. Результаты работы программы в виде графика (2)
Теперь рассмотрим, как использовать показания гироскопа. В отличие от акселерометра показания гироскопа не привязаны к определенной позиции, это только текущая угловая скорость. Для того, чтобы получить при помощи нее значение угла поворота, нужно знать предыдущее значение угла и добавить к нему изменение угла, которое есть произведение угловой скорости по выбранной оси на время в течении которого данная скорость действовала. Ниже приведен пример использования гироскопа для подсчета изменения угла.
Рисунок 12. Программа работы с гироскопическими данными
Но, если внимательно изучить результат работы данной программы, становится очевидным, что даже при неподвижном состоянии полученные углы изменяются, этот эффект называется смещением нуля гироскопа и может быть легко устранен при помощи калибровки. В результате калибровки создаются три значения (калибровочные константы по каждой оси), которые затем отнимаются от значений, полученных от гироскопа.
За калибровку в библиотеке MPU6050.h отвечает функция calibrateGyro(). Ниже приведен пример программы с использованием калибровки. Отмечу, что после включения, стенд (MPU-6050) в течение не менее 5 секунд должен быть неподвижен, в это время производится опрос гироскопа, и накопленная погрешность пересчитывается в качестве калибровочной. Всю программу я приводить не буду, изменения коснулись только функции setup.
Рисунок 13. Программа калибровки гироскопа
Запустите полученную программу, теперь, после калибровки, изменения угла при наклонах MPU-6050 стали много точнее, но все равно заметно накопление погрешности. К сожалению, при использовании только гироскопа, эту погрешность полностью устранить не возможно, она связана с дискретностью опроса прибора. Но, если объединить результаты работы гироскопа и акселерометра, добиться точных результатов можно.
Наиболее простым и эффективным способом объединения значений от электронного гироскопа и акселерометра является комплементарный фильтр.
, (3)
где A – результирующий угол (в скобках, этот угол из предыдущей итерации расчета), Asp – текущая угловая скорость, dt – время от предыдущего опроса и расчета угла, Aac – угол, рассчитанный по показаниям акселерометра, 0.99 и 0.01 в сумме дают 1.0 и являются коэффициентами пропорциональности участия гироскопа и акселерометра в получении результата. Коэффициенты пропорциональности можно в небольшой степени изменять для достижения наилучшего результата. А ниже приведен результат работы нашей программы при плавном наклоне гироприбора – шумов нет, изменения углов происходит плавно.
Рисунок 14. Данные с гироскопа (стенд наклоняется)
А здесь гироприбор неподвижен, и наклонен на ~0.80. Основные измерения производятся гироскопом, а при помощи акселерометра мы устраняем дрейф нуля.
Рисунок 15. Данные с гироскопа (стенд неподвижен)
Сравнение полученного угла по данным только гироскопа (синяя линия) и при помощи комплементарного фильтра (красная линия), данных от акселерометра (зеленая линия). После нескольких секунд работы данные рассчитанные только гироскопом начинают накапливать отклонения…
Рисунок 16. Комплексные данные
Отлично!!! Мы адаптировали MPU-6050 под свои нужды, теперь рассмотрим BNO055.
Как я отметил, BNO055 содержит микроконтроллер и может сам производить расчеты, подобные тем, что мы приводили выше, когда рассматривали MPU-6050. После инициализации он начинает производить расчеты, и нам можно по необходимости, запрашивать и получать готовые результаты, например, углы наклона.
При программировании будем пользоваться библиотекой Adafruit_BNO055.h и классами, разработанными для работы с векторами см. http://zizibot.ru/directory/sensor/bno055/.
Проверим, каковы результаты запроса к BNO055 для акселерометра, если мы рассчитываем угол самостоятельно.
Рисунок 17. Программа работы с BNO055 (вывод данных акселерометра)
Рисунок 18. Данные от акселерометра BNO055 (Визуально видно, что колебания ниже, но они есть)
Изменим программу таким образом, чтобы запрашивать не результаты работы акселерометра, а отфильтрованные по гравитации, изменив параметр вызова запроса данных от гироприбора.
Рисунок 19. Программа работы с BNO055 (данные фильтра BNO055)
Как видно из рисунка, результаты значительно лучше, но небольшие колебания присутствуют, из чего можно сделать вывод, что получение результата на основе работы комплементарного фильтра от угла акселерометра и угловой скорости гироскопа чище.
Рисунок 20. Данные фильтра BNO055
Теперь воспользуемся возможностью BNO055 по расчету углов Эйлера (крен, тангаж, рыскание). Это как раз те самые углы, расчетом которых мы занимались.
Рисунок 21. Программа (рассчитываем углы Эйлера)
Анализ значений показывает, что углы Эйлера посчитанные внутри BNO055 и углы посчитанные при помощи вектора гравитации почти совпадают. Из этого можно сделать вывод, что данные от гироскопа уже присутствуют в расчетах вектора гравитации и Эйлеровых углов, но для чистоты эксперимента составим комплементарный фильтр и на его основе рассчитаем результирующий гол наклона.
Ниже три значения: Углы Эйлера,
Рисунок 22. Плавная линия - углы Эйлера
Ниже: синяя линия расчет от BNO055 углов Эйлера, красная линия комплементарный фильтр из данных гороскопа и акселерометра.
Рисунок 23. Программа (рассчитываем углы Эйлера, комплементарного фильтра)
Рисунок 24. Результат работы программа (углы Эйлера, комплементарного фильтра)
И последнее (ниже), используем вместо данных акселерометра данные вектора гравитации
Рисунок 25. Программа (вектор гравитации)
Синяя линия – угол Эйлера рассчитанный внутри BNO055, красная линия комплементарный фильтр от данных вектора гравитации и гироскопа. Можно видеть, что при запросе углов Эйлера встречаются ситуации, когда предыдущий и последующий (через 5-10 миллисекунд) углы совпадают, можно сделать вывод, что период в 5мсек недостаточен для гарантированного пересчета углов, и в регистрах данных BNO055 данные не успевают измениться.
Рисунок 26. Результат работы программа (синяя линия – угол Эйлера рассчитанный внутри BNO055, красная линия комплементарный фильтр от данных вектора гравитации и гироскопа)
Выводы. Мы изучили работу гироприборов BNO055 и MPU5060, рассмотрели особенности их использования, составили алгоритмы, которые будут использованы при построении балансирующего робота.
ОСТАВИТЬ КОММЕНТАРИЙ
Форма авторизации
ВОЙТИ С ПОМОЩЬЮ:
ИЛИ Авторизация на сайте: